/******************************************************************************
 * Copyright 2022 The Airos Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "rsi_generator_component.h"

#include <glog/logging.h>

#include "gflags/gflags.h"
#include "middleware/protocol/common/global_conf.h"
#include "middleware/protocol/rsi_generator/include/v2x-asn-msgs-adapter.hpp"

namespace os {
namespace v2x {
namespace protocol {

DEFINE_int32(rsu_intersection_id, 10, "rsu intersection id");
DEFINE_string(city_string, "gzho#", "city name with length is 5 and end with #");
DEFINE_string(asn_message_version, "4layer", "v2x message Asn.1 file version");
DEFINE_string(rsu_xml_map_file, "/home/caros/cybertron/rsu_map.xml", "xml map file name");

int AIROS_COMPONENT_CLASS_NAME(RsiGeneratorComponent)::msg_cnt_ = 0;

bool AIROS_COMPONENT_CLASS_NAME(RsiGeneratorComponent)::GetRsuMap(const std::string& rsu_map) {
  std::string asn_version(FLAGS_asn_message_version);
  EnAsnType asn_type{EnAsnType::CASE_53_2020};
  if (asn_version.compare("4layer") == 0) {
    asn_type = EnAsnType::CASE_53_2020;
  } else if (asn_version.compare("new_4layer") == 0) {
    asn_type = EnAsnType::YDT_3709_2020;
  } else {
    // pass
  }

  std::fstream fs(rsu_map.data(), std::fstream::in);
  if (!fs.good()) {
    AWARN << "xml file open false:" << rsu_map;
    return false;
  }
  std::stringstream ss;
  ss << fs.rdbuf();
  fs.close();
  std::string content(ss.str());

  if (content.empty()) {
    AWARN << "xml map data null";
    return false;
  }
  std::string str_asn("");
  if (0 >= message_frame_map_xml2uper_adapter(content, &str_asn, asn_type)) {
    AWARN << " map xml to uper:fasle";
    return false;
  }

  std::string str_pb("");
  if (0 >= message_frame_uper2pbstr_adapter(str_asn, &str_pb, asn_type)) {
    AWARN << "asn to pb false";
    return false;
  }
  AINFO << "asn to pb true";

  asn_map_data_ = std::make_shared<v2xpb::asn::MessageFrame>();
  if (asn_map_data_) {
    asn_map_data_->ParsePartialFromString(str_pb);
  } else {
    AWARN << "map pb null";
    return false;
  }
  if (asn_map_data_->payload_case() != v2xpb::asn::MessageFrame::PayloadCase::kMapFrame) 
  {
    AERROR << "MAP PB get failed.";
    return false;
  }
  AINFO << asn_map_data_->DebugString();
  auto asn_map = asn_map_data_->mapframe();
  if (asn_map.nodes_size() == 0) {
    AWARN << "MAP WRONG.";
    return false;
  }
  auto nodes = asn_map.nodes(0);
  if (nodes.has_position() && nodes.position().has_llh()) {
    auto pos = nodes.position().llh();
    cross_lat_ = pos.latitude();
    cross_lon_ = pos.longitude();
  }
  AWARN << "cross_lat : " << cross_lat_ << " cross_lon : " << cross_lon_;
  return true;
}

bool AIROS_COMPONENT_CLASS_NAME(RsiGeneratorComponent)::Init() {
  auto global_conf = GlobalConf::Instance();
  rscu_sn_ = global_conf->GetRscuSn();
  if (!LoadConfig(&conf_)) {
    AERROR << "load component proto config error";
    return false;
  }
  for (int ev_num = 0; ev_num < conf_.event_detail_size(); ++ev_num) {
    auto ev_detail = conf_.event_detail(ev_num);
    if(!ev_detail.has_perception_name()) {
        continue;
    }
    RteEventDetailPtr rte_event_ptr(new v2xpb::rscu::config::RteEventDetail(ev_detail.rte_detail()));
    ev_map_[ev_detail.perception_name()] = rte_event_ptr;
  }
  affect_path_ = std::make_shared<RsiAffectPath>();
  rsu_intersection_id_ = FLAGS_rsu_intersection_id;
  city_string_ = FLAGS_city_string;
  zone_ = 31 + cross_lon_ / 6;
  std::string map_path = FLAGS_rsu_xml_map_file;
  if (!GetRsuMap(map_path)) {
    AERROR << "Get Rsu Map Failed.";
    return false; 
  }
  return true;
}

bool AIROS_COMPONENT_CLASS_NAME(RsiGeneratorComponent)::Proc(
  const std::shared_ptr<const airos::usecase::EventOutputResult>& event_ptr) {
  if (event_ptr->ByteSizeLong() == 0) {
    AERROR << "v2xobstacles recv data is nullptr.";
    return false;
  }
  int rte_count = 0;
  AINFO << event_ptr->DebugString();
  v2xpb::asn::Rsi* rsi_pb_data;
  for (int index = 0; index < event_ptr->events_size(); ++index) {
    auto event_info = event_ptr->events(index);
    auto v2x_type = event_info.event_type();
    std::string event_name = airos::usecase::EventInformation_EventType_Name(v2x_type);
    AINFO << "Event Ocurred, event type: " << event_name;
    if (ev_map_.find(event_name) == ev_map_.end()) {
      AINFO << "UNDEFINED event: " << event_name;
      continue;
    }
    auto ev_detail = ev_map_[event_name];
    if (!ev_detail->has_priority()) {
      AINFO << "CONFIG has no type id or description.";
      continue;
    }
    if (rte_count == 0) {
      asn_pb_data_ = std::make_shared<v2xpb::asn::MessageFrame>();
      rsi_pb_data = asn_pb_data_->mutable_rsiframe();
    }
    auto v2x_rte = rsi_pb_data->add_rtes();
    v2x_rte->set_event_type(ev_detail->event_type_id());
    v2x_rte->set_priority(ev_detail->priority());
    v2x_rte->set_rte_id(rte_count);
    v2x_rte->set_event_source(v2xpb::asn::RsiRte_Source_SRC_DETECTION);
    v2x_rte->set_event_radius(25);
    auto position = v2x_rte->mutable_position();
    auto position_xyz = position->mutable_xyz();
    position_xyz->set_zone(zone_);
    position_xyz->set_x(event_info.location_point().x());
    position_xyz->set_y(event_info.location_point().y());
    airos::perception::usecase::Vec2d vec2d(
      event_info.location_point().x(), event_info.location_point().y());
    std::vector<v2xpb::asn::RsiReferencePath> affect_paths;
    affect_path_->GetAffectPath(vec2d, asn_map_data_->mapframe(), &affect_paths);
    for (auto affect_path : affect_paths) {
      v2x_rte->add_ref_paths()->operator=(affect_path);
    }
    v2x_rte->set_description(ev_detail->description());
    rte_count++;
    if (rte_count == 8) {
      GenerateRsiMsg(rsi_pb_data, msg_cnt_);
      msg_cnt_++;
      if(msg_cnt_ > 127) {
        msg_cnt_ = 0;
      }
      AINFO << asn_pb_data_->DebugString();
      Send("/v2x/message/generated", asn_pb_data_);
      rsi_pb_data = nullptr;
      rte_count = 0;
    }
  }
  if (rte_count > 0) {
    GenerateRsiMsg(rsi_pb_data, msg_cnt_);
    msg_cnt_++;
    if(msg_cnt_ > 127) {
      msg_cnt_ = 0;
    }
    AINFO << asn_pb_data_->DebugString();
    Send("/v2x/message/generated", asn_pb_data_);
    rsi_pb_data = nullptr;
    rte_count = 0;
  }
  return true;
}

void AIROS_COMPONENT_CLASS_NAME(RsiGeneratorComponent)::GenerateRsiMsg(
  v2xpb::asn::Rsi* rsi_pb, int msg_cnt) {
  if (rsu_intersection_id_ < 10) {
    rsi_pb->set_rsi_id(city_string_ + "00" +
        std::to_string(rsu_intersection_id_));
  } else if (rsu_intersection_id_ < 100) {
    rsi_pb->set_rsi_id(city_string_ + "0" +
        std::to_string(rsu_intersection_id_));
  } else {
    rsi_pb->set_rsi_id(city_string_ +
        std::to_string(rsu_intersection_id_));
  }
  rsi_pb->set_message_count(msg_cnt);
  auto ref_llh = rsi_pb->mutable_position()->mutable_llh();
  ref_llh->set_latitude(cross_lat_);
  ref_llh->set_longitude(cross_lon_);
}

}  // namespace protocol
}  // namespace v2x
}  // namespace os
